#include "foc.h"


/**
  * @brief  生成三相正弦波
  * @param  sineWave: 正弦波输出结构体指针
  * @param  electricalAngle: 电角度指针(rad)，函数会更新此值
  * @retval 无
  */

void ThreePhaseSineWave_Generate(SineWave_t *sineWave, float *CHUXIANG)
{
	static float Awt;
	static float Bwt;
	static float Cwt;
	static float angleStep;
	
	Awt = *CHUXIANG;
	Bwt = *CHUXIANG - 2.0f/3.0f * PI;
	Cwt = *CHUXIANG - 4.0f/3.0f * PI;
	
	sineWave->Phase_A = A*sinf(Awt);
	sineWave->Phase_B = A*sinf(Bwt);
	sineWave->Phase_C = A*sinf(Cwt);
	
	angleStep = 2* PI * Fre / Amp;
	
	*CHUXIANG += angleStep;
	if(*CHUXIANG>=2*PI)
	{
		*CHUXIANG-=2*PI;
	}
}

/**
  * @brief  Clarke变换（三相静止到两相静止坐标系）
  * @param  clarke: Clarke变换结果结构体指针
  * @param  motorParams: 电机参数结构体指针
  * @retval 无
  */
void ClarkeTransform(Clarke_Transform *clarke , Motor_RealTimeParams *motorParams)
{
	clarke->current_alpha = motorParams->Current_A*2/3-(motorParams->Current_B + motorParams->Current_C)/3;
	clarke->current_beta  = (motorParams->Current_B - motorParams->Current_C)*sqrt(3)/3;
}





/**
  * @brief  Park变换（两相静止到两相旋转坐标系）
  * @param  clarke: Clarke变换结果结构体指针
  * @param  motorParams: 电机参数结构体指针
  * @param  park: Park变换结果结构体指针
  * @retval 无
  */
void ParkTransform(Clarke_Transform *clarke , Motor_RealTimeParams *motorParams,Park_Transform *PARK)
{
	PARK->current_D = clarke->current_alpha * cosf(motorParams->E_angle) + clarke->current_beta * sinf(motorParams->E_angle);
	PARK->current_Q = -clarke->current_alpha * sinf(motorParams->E_angle) + clarke->current_beta * cosf(motorParams->E_angle);
 }

 
 
 
 
 /**
  * @brief  逆Park变换（两相旋转到两相静止坐标系）
  * @param  voltageD: D轴电压
  * @param  voltageQ: Q轴电压
  * @param  motorParams: 电机参数结构体指针
  * @param  inversePark: 逆Park变换结果结构体指针
  * @retval 无
  */
void InverseParkTransform_init(float UD, float UQ, Motor_RealTimeParams *motorParams , InversePark_Transform *inversePark)
{
	inversePark->Voltage_alpha = UD * cosf(motorParams->E_angle) - UQ * sinf(motorParams->E_angle);
	inversePark->Voltage_beta  = UD * sinf(motorParams->E_angle) + UQ * cosf(motorParams->E_angle);
}






/**
  * @brief  逆Clarke变换（两相静止到三相静止坐标系）
  * @param  inverseClarke: 逆Clarke变换结果结构体指针
  * @param  inversePark: 逆Park变换结果结构体指针
  * @retval 无
  */
void InverseClarkeTransform_init(InverseClarke_Transform *inverseClarke , InversePark_Transform *inversePark)
{
	inverseClarke->Voltage_A = inversePark->Voltage_alpha;
	inverseClarke->Voltage_B = inversePark->Voltage_alpha * (-1.0f/2.0f) + inversePark->Voltage_beta * sqrt(3)/2;
	inverseClarke->Voltage_C = inversePark->Voltage_alpha * (-1.0f/2.0f) - inversePark->Voltage_beta * sqrt(3)/2;
}






/**
  * @brief  空间矢量脉宽调制(SVPWM)
  * @param  inverseClarke: 逆Clarke变换结果结构体指针
  * @param  svpwm: SVPWM输出结构体指针
  * @param  motorParams: 电机参数结构体指针
  * @retval 无
  */
void SpaceVectorPWM(InverseClarke_Transform *inversePark , SVPWM *Svpwm, Motor_RealTimeParams *motorParams)
{
	float V_max = fmaxf(inversePark->Voltage_A , fmaxf(inversePark->Voltage_B , inversePark->Voltage_C));
	float V_min = fminf(inversePark->Voltage_A , fminf(inversePark->Voltage_B , inversePark->Voltage_C));
	
	float V0 = (-1.0f/2.0f) * (V_max + V_min);
	
	float tempA = inversePark->Voltage_A + V0;
	float tempB = inversePark->Voltage_B + V0;
	float tempC = inversePark->Voltage_C + V0;

	Svpwm->A_period = ( tempA / motorParams->DC_Voltage +0.5f)  * Svpwm->Tpwm;
	Svpwm->B_period = ( tempB / motorParams->DC_Voltage +0.5f)  * Svpwm->Tpwm;
	Svpwm->C_period = ( tempC / motorParams->DC_Voltage +0.5f)  * Svpwm->Tpwm;
}


//开环角度生成函数  
void angle_Generate(Angle_Gerate_Memory *Angle_Memory,Motor_State_Params *M_Memory)
{
	//定义机械角度增量
	float Mechine_Angle_Add = 0.0f;
	//定义电角度增量
	float Elec_Angle_Add = 0.0f;
	
	//将设置的机械转速转换为机械角频率
	Angle_Memory->Mechanical_Freqnence = Angle_Memory->open_Speed_Freqnence * 2 * PI;
    
	//计算机械角度增量
    Mechine_Angle_Add = Angle_Memory->Mechanical_Freqnence * Angle_Memory->Sample_Period;
	
	//计算电角度增量
	Elec_Angle_Add = Mechine_Angle_Add * M_Memory->Motor_Poler;
	
	//更新机械角度
	Angle_Memory->Mechanical_Angle += Mechine_Angle_Add;
	
	//机械角度限幅度
	if (Angle_Memory->Mechanical_Angle > 2 *PI)
	{
		Angle_Memory->Mechanical_Angle -= 2 *PI;
	}
	else if(Angle_Memory->Mechanical_Angle < 0)
	{
		Angle_Memory->Mechanical_Angle += 2 * PI; 
	} 
	
	//更新电角度
	Angle_Memory->Eelectrical_Angle += Elec_Angle_Add;
	
	//电角度限幅度
	if (Angle_Memory->Eelectrical_Angle > 2 *PI)
	{
		Angle_Memory->Eelectrical_Angle -= 2 *PI;
	}
	else if(Angle_Memory->Eelectrical_Angle < 0)
	{
		Angle_Memory->Eelectrical_Angle += 2 * PI; 
	} 
}


//void PI_Calculate(PI_Controller *pid, float setpoint, float actual) {
//    // 1. 计算当前误差
//    float error = setpoint - actual;

//    // 2. 比例项
//    float proportional_term = pid->Kp * error;

//    // 3. 积分项（先累加）
//    pid->IntegralSum += error;
//    float integral_term = pid->Ki * pid->IntegralSum;

//    // 4. 计算总输出
//    pid->Output = proportional_term + integral_term;

//    // 5. 输出限幅 (Anti-Windup:  clamping)
//    if (pid->Output > pid->OutMax) {
//        pid->Output = pid->OutMax;
//        // 积分饱和处理：如果输出饱和，则停止积分累加（或只累加负误差）
//    } else if (pid->Output < pid->OutMin) {
//        pid->Output  = pid->OutMin;
//	}
//}

void PI_Calculate(PI_Controller *pid, float setpoint, float actual) {
    // 1. 计算当前误差
    float error = setpoint - actual;

    // 2. 比例项
    float proportional_term = pid->Kp * error;

    // 3. 积分项 - 先更新积分器，然后立即进行限幅
    pid->IntegralSum += error;
    
    // 积分限幅：防止积分项变得过大
    // 计算积分项的合理限幅范围，通常基于输出限幅和Ki值
    // 例如：积分限幅 = 输出限幅 / Ki，这样可以确保积分项不会单独导致输出饱和
    float integral_max = pid->OutMax / pid->Ki;
    float integral_min = pid->OutMin / pid->Ki;
    
    // 应用积分限幅
    if (pid->IntegralSum > integral_max) {
        pid->IntegralSum = integral_max;
    } else if (pid->IntegralSum < integral_min) {
        pid->IntegralSum = integral_min;
    }
    
    float integral_term = pid->Ki * pid->IntegralSum;

    // 4. 计算总输出
    pid->Output = proportional_term + integral_term;

    // 5. 输出限幅
    if (pid->Output > pid->OutMax) {
        pid->Output = pid->OutMax;
    } else if (pid->Output < pid->OutMin) {
        pid->Output = pid->OutMin;
    }
}

void LQF_Filter(LQF *p , float inout)
{
	p->OUTPUT = p->LQFK * inout + ( 1 - p->LQFK ) * p->OUTPUT;
}






/*滑膜观测器*/


void SMO_Oberserve(SMO_observe *SMO,float U_Alpha , float U_Beta , float I_Alpha , float I_Beta , Motor_State_Params *motor)
{
	//计算电流误差(估算值 - 实际值)
	float I_Alpha_Eeeor = SMO->I_Alpha_Hat - I_Alpha;  //α轴电流误差
	float I_Beta_Error = SMO->I_Beta_Hat - I_Beta ;   //β轴电流误差
	
	//修正项
	float Z_Alpha = SMO ->SMO_K * Saturation(I_Alpha_Eeeor , 2.0f);   //α轴切换项
	float Z_Beta = SMO ->SMO_K * Saturation(I_Beta_Error , 2.0f);   //β轴切换项
	
	//估算电流更新
	// 电压方程：U = Rs*I + Ls*(di/dt) + E（E为反电动势，这里用Z近似）
    // 推导得：di/dt = (U - Rs*I_estimated - Z) / Ls}
	float Di_Alpha_Dt = (1.0f / motor->Motor_Ls) * (U_Alpha - motor->Motor_Rs * SMO->I_Alpha_Hat - Z_Alpha);
	float Di_Beta_Dt = (1.0f / motor->Motor_Ls) * (U_Beta - motor->Motor_Rs * SMO->I_Beta_Hat - Z_Beta);

	//反电动势滤波（低通滤波，平滑切换项中的高频分量）
	SMO->E_Alpha_Filter += SMO->E_LQF * (Z_Alpha - SMO->E_Alpha_Filter) * motor->Motir_Ts;
	SMO->E_Beta_Filter += SMO->E_LQF * (Z_Beta - SMO->E_Beta_Filter) * motor->Motir_Ts;
	
	// 积分更新估算电流（欧拉法：i += di/dt * Ts）
	SMO->I_Alpha_Hat += Di_Alpha_Dt * motor->Motir_Ts;
	SMO->I_Beta_Hat += Di_Beta_Dt * motor->Motir_Ts;	
}


void PLL_Oberserver(PLL *Pll , float E_Alpha , float E_Beta , float Kp , float Ki , float We_limit, Motor_State_Params *motor)
{
	//计算误差 err = -e_alpha * cos(theta) - e_beta * sin(theta) 生成角度与实际角度之差
	Pll->Error_Theta = -E_Alpha * cosf(Pll->Theta_Hat_Pervious) - E_Beta * sinf(Pll->Theta_Hat_Pervious);
	
	//PI控制器计算
	//计算积分项
	Pll->Integral_Term += Ki * Pll->Error_Theta;
	
	//计算电 角速度
	Pll->We = (Kp * Pll->Error_Theta + Pll->Integral_Term);
	
	//电 角速度限幅
	if(Pll->We > We_limit)
	{
		Pll->We = We_limit;
	}
	else if (Pll->We < -We_limit)
	{
		Pll->We = -We_limit;
	}
	
	//速递滤波
	Pll->We_Filter = Pll->We * Pll->PLL_LQF + Pll->We_Filter * (1 - Pll->PLL_LQF);
	
	//计算电角度
	Pll->Theta_Hat += Pll->We_Filter * motor->Motir_Ts;
	
	//角度限幅
	if(Pll->Theta_Hat > 2*PI)
	{
		Pll->Theta_Hat -= 2*PI;
	}
	else if(Pll->Theta_Hat < 0)
	{
		Pll->Theta_Hat += 2 * PI;
	}
	
	Pll->Theta_Hat_Pervious = Pll->Theta_Hat;
}




//饱和函数(滑膜核心)

float Saturation(float Val , float Phi)
{
	if(Phi <= 0.0f)
	{
		//如果phi无效（<=0）,退化为符号函数
		if(Val>0.0f)return 1.0f;
		if(Val<0.0f)return -1.0f;
		return 0;
	}
	if(Val > Phi)
	{
		return 1.0f;
	}
	else if(Val < -Phi)
	{
		return -1.0f;
	}
	else
	{
		return Val / Phi;
	}
}	

//初始化磁链观测器
void Init_Flux_Observe(Flux_Memory *Flux_Memory)
{
	Flux_Memory->Active_Flux = 0;
    Flux_Memory->Error_Mpy_Gain = 0;
	Flux_Memory->Eta_X_Alpha = 0;
	Flux_Memory->Eta_X_Beta = 0;
	Flux_Memory->Eta_x_s = 0;
	Flux_Memory->Flux_Active_Part = 0;
	Flux_Memory->Flux_Alpha = 0;
	Flux_Memory->Flux_Beta = 0;
	Flux_Memory->Flux_Error = 0;
	Flux_Memory->Flux_Mechanical_Speed = 0;
	Flux_Memory->Flux_Omega = 0;
	Flux_Memory->Flux_s = 0;
	Flux_Memory->Flux_Theta = 0;
	Flux_Memory->Inter_Input_Alpha = 0;
	Flux_Memory->Inter_Input_Beta = 0;
	Flux_Memory->Motor_Flux = 0.0055133;
    Flux_Memory->Motor_Ld = 0.3;
    Flux_Memory->Motor_Lq = 0.3;
    Flux_Memory->Motor_Rs = 0.6;
    Flux_Memory->Observer_Gain = 2000;
    Flux_Memory->PI_Inter = 0;
    Flux_Memory->PI_Inter_Limit = 1500;
	Flux_Memory->PI_Out_Limit = 1500;
	Flux_Memory->PLL_Error = 0;
	Flux_Memory->PLL_Ki = 0.1;
	Flux_Memory->PLL_Kp = 0.1;
	Flux_Memory->PLL_Out_Limt = 1500;
	Flux_Memory->Ts = 0.00005;
	Flux_Memory->X_Alpha = 0;
	Flux_Memory->X_Beta = 0;
	Flux_Memory->Y_Alpha = 0;
	Flux_Memory->Y_Beta = 0;
	Flux_Memory->Filter_Speed = 0;
    Flux_Memory->LQF_K = 1.0;   	
}



//磁链观测器
void Flux_PLL_Observe(Flux_Memory *Flux_Memory,float I_Alpha,float I_Beta,float U_Alpha,float U_Beta,float ID)
{
	  // 计算反电动势alpha/beta轴分量（Y = V - R*I）
	  Flux_Memory->Y_Alpha = U_Alpha - Flux_Memory->Motor_Rs * I_Alpha;
	  Flux_Memory->Y_Beta  = U_Beta  - Flux_Memory->Motor_Rs * I_Beta;
	
	  // 计算转子磁链中间变量（Eta_X = X - Lq*I）
	  Flux_Memory->Eta_X_Alpha = Flux_Memory->X_Alpha - Flux_Memory->Motor_Lq *  I_Alpha;
	  Flux_Memory->Eta_X_Beta = Flux_Memory->X_Beta   - Flux_Memory->Motor_Lq *  I_Beta;
	  
	  // 计算有效磁链分量（考虑d/q轴电感差异）
	  Flux_Memory->Flux_Active_Part = ID * (Flux_Memory->Motor_Ld - Flux_Memory->Motor_Lq);
	 
	  // 计算总有效磁链（基础磁链 + 电感差异引起的分量）
	  Flux_Memory->Active_Flux = Flux_Memory->Motor_Flux +  Flux_Memory->Flux_Active_Part;
	
	  // 计算转子磁链合成值的平方（Eta_X_s = Eta_X_alpha2 + Eta_X_beta2）
	  Flux_Memory->Eta_x_s = Flux_Memory->Eta_X_Alpha *  Flux_Memory->Eta_X_Alpha +  Flux_Memory->Eta_X_Beta *  Flux_Memory->Eta_X_Beta;
	
	 // 计算磁链误差（目标磁链平方 - 实际磁链平方）
	 Flux_Memory->Flux_Error = Flux_Memory->Active_Flux * Flux_Memory->Active_Flux - Flux_Memory->Eta_x_s;
	
	  // 误差乘以观测器增益
	 Flux_Memory->Error_Mpy_Gain =  Flux_Memory->Flux_Error * Flux_Memory->Observer_Gain;
	 
	   // 计算积分器输入（反电动势 + 误差反馈项）
	 Flux_Memory->Inter_Input_Alpha = Flux_Memory->Y_Alpha +  Flux_Memory->Eta_X_Alpha *  Flux_Memory->Error_Mpy_Gain;
	 Flux_Memory->Inter_Input_Beta  = Flux_Memory->Y_Beta  +  Flux_Memory->Eta_X_Beta  *  Flux_Memory->Error_Mpy_Gain;
	   
	 // 积分更新定子磁链（X = ∫(积分器输入)dt，用前向欧拉法近似积分）
	 Flux_Memory->X_Alpha  += Flux_Memory->Inter_Input_Alpha * Flux_Memory->Ts;
	 Flux_Memory->X_Beta   += Flux_Memory->Inter_Input_Beta * Flux_Memory->Ts;
	
	 // 计算转子磁链alpha/beta轴分量（Flux = X - Lq*I）
	 Flux_Memory->Flux_Alpha = Flux_Memory->X_Alpha - Flux_Memory->Motor_Lq * I_Alpha;
	 Flux_Memory->Flux_Beta  = Flux_Memory->X_Beta  - Flux_Memory->Motor_Lq * I_Beta;
	 
	 // 计算转子磁链合成值（模长）
	 Flux_Memory->Flux_s = sqrtf(Flux_Memory->Flux_Alpha * Flux_Memory->Flux_Alpha + Flux_Memory->Flux_Beta * Flux_Memory->Flux_Beta);
	  
	 // 磁链合成值限幅（防止后续除法运算除以0）
	if(Flux_Memory->Flux_s < 0.0001f)
	{
		Flux_Memory->Flux_s = 0.0001f;
	}
	else if(Flux_Memory->Flux_s > 0.0055133)
	{
		Flux_Memory->Flux_s = 0.0055133;
	}
	
	 // 计算锁相环误差输入（基于磁链定向的相位误差）
	  // 公式：-Flux_alpha*sin(theta) + Flux_beta*cos(theta)
	 float Sin_Theta = sinf(Flux_Memory->Flux_Theta);
	 float Cos_Theta = cosf(Flux_Memory->Flux_Theta);
	 Flux_Memory->PLL_Error = -Flux_Memory->Flux_Alpha * Sin_Theta + Flux_Memory->Flux_Beta * Cos_Theta;
	 Flux_Memory->PLL_Error /= Flux_Memory->Flux_s;
	
	 // 步骤11：PI控制器（物理单位参数）
	 float PI_Error = 0.0f - Flux_Memory->PLL_Error;
	
	 // 积分项计算（使用实际时间步长）
	  Flux_Memory->PI_Inter +=  Flux_Memory->PLL_Ki * PI_Error;
	
	// 积分限幅
	 if(Flux_Memory->PI_Inter > Flux_Memory->PI_Inter_Limit)
	 {
		 Flux_Memory->PI_Inter = Flux_Memory->PI_Inter_Limit;
	 }
	 else if(Flux_Memory->PI_Inter < -Flux_Memory->PI_Inter_Limit)
	 {
		  Flux_Memory->PI_Inter = -Flux_Memory->PI_Inter_Limit;
	 }
	
	  // 输出计算
	 float PI_Out = Flux_Memory->PLL_Kp * PI_Error + Flux_Memory->PI_Inter;
	 
	  // 输出限幅
	 if(PI_Out > Flux_Memory ->PI_Out_Limit)
	 {
		 PI_Out = Flux_Memory ->PI_Out_Limit;
	 }
	 else if(PI_Out < - Flux_Memory ->PI_Out_Limit)
	 {
		  PI_Out = -Flux_Memory ->PI_Out_Limit;
	 }
	 
	  // 步骤12：更新角速度和角度
	 Flux_Memory->Flux_Omega = PI_Out;
	 
	 // 角度更新：积分角速度×时间
	 Flux_Memory->Flux_Theta += Flux_Memory->Flux_Omega * Flux_Memory->Ts;
	 
	   // 角度范围调整到[0, 2π)
	if(Flux_Memory->Flux_Theta > 2 * PI)
	{
		Flux_Memory->Flux_Theta-= 2 * PI;
	}
	else if(Flux_Memory->Flux_Theta < 0)
	{
		Flux_Memory->Flux_Theta += 2*PI;
	}
	
	//计算机械转速 (rad/s) = 电角速度 / 极对数
	Flux_Memory->Flux_Mechanical_Speed = Flux_Memory->Flux_Omega * 2.387;
	
    //低通滤波器 转速滤波
    Flux_Memory->Filter_Speed = Flux_Memory->LQF_K * Flux_Memory->Flux_Mechanical_Speed + (1 - Flux_Memory->LQF_K) * Flux_Memory->Flux_Mechanical_Speed;
	
}

